{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "为了实现这个方法并可视化过程，我们可以使用Python的numpy库来计算变换矩阵，并使用matplotlib库来可视化结果。首先，我们需要导入所需的库："
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "from mpl_toolkits.mplot3d import Axes3D\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "from mpl_toolkits.mplot3d import Axes3D\n",
    "\n",
    "# 创建一个3D坐标系\n",
    "fig = plt.figure()\n",
    "ax = fig.add_subplot(111, projection='3d')\n",
    "\n",
    "# 定义坐标轴\n",
    "ax.set_xlabel('X-axis')\n",
    "ax.set_ylabel('Y-axis')\n",
    "ax.set_zlabel('Z-axis')\n",
    "\n",
    "# 定义姿态\n",
    "x1, y1, z1, w, x2, y2, z2 = 1, 2, 3, 1, 4, 5, 6\n",
    "\n",
    "# 计算旋转矩阵\n",
    "R = np.array([[w**2 + x1**2 - y1**2 - z1**2, 2*(x1*y1 - w*z1), 2*(x1*z1 + w*y1)],\n",
    "              [2*(x1*y1 + w*z1), w**2 + y1**2 - x1**2 - z1**2, 2*(y1*z1 - w*x1)],\n",
    "              [2*(x1*z1 - w*y1), 2*(y1*z1 + w*x1), w**2 + z1**2 - x1**2 - y1**2]])\n",
    "\n",
    "# 计算旋转后的坐标\n",
    "p_rot = np.dot(R, np.array([x2, y2, z2]))\n",
    "\n",
    "# 绘制坐标系\n",
    "ax.plot([0, x1], [0, y1], [0, z1], 'r')\n",
    "ax.plot([x1, x2], [y1, y2], [z1, z2], 'g')\n",
    "\n",
    "# 绘制姿态\n",
    "ax.scatter(x1, y1, z1, c='r', marker='o')\n",
    "ax.scatter(x2, y2, z2, c='g', marker='o')\n",
    "\n",
    "# 绘制X轴，用红色线\n",
    "ax.plot([x1, x1 + 1], [y1, y1], [z1, z1], 'r')\n",
    "# 绘制Y轴，用绿色线\n",
    "ax.plot([x1, x1], [y1, y1 + 1], [z1, z1], 'g')\n",
    "# 绘制Z轴，用蓝色线\n",
    "ax.plot([x1, x1], [y1, y1], [z1, z1 + 1], 'b')\n",
    "\n",
    "# 设置坐标轴范围\n",
    "ax.set_xlim(-7, 7)\n",
    "ax.set_ylim(-7, 7)\n",
    "ax.set_zlim(-7, 7)\n",
    "\n",
    "# 显示图形\n",
    "plt.show()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [],
   "source": [
    "import os\n",
    "import sys\n",
    "sys.path.append(os.path.abspath('.'))\n",
    "from my_math import combine_frame_transforms, subtract_frame_transforms\n",
    "import torch"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "tensor([0, 0, 0, 1, 0, 0, 0])\n",
      "tensor([254,   0,  50,   1,   0,   0,   0])\n",
      "combine_frame: (tensor([254,   0, 100]), tensor([1., 0., 0., 0.]))\n"
     ]
    }
   ],
   "source": [
    "base = torch.tensor([0,0,0,1,0,0,0,0])\n",
    "\n",
    "base = torch.tensor([0,0,0,1,0,0,0])\n",
    "print(base)\n",
    "ee_frame = torch.tensor([45,49,0,1,0,0,0])\n",
    "print(ee_frame)\n",
    "\n",
    "object_frame = torch.tensor([0,0,50,1,0,0,0])\n",
    "\n",
    "combine_frame = combine_frame_transforms(\n",
    "    t01=ee_frame[:3], q01=ee_frame[3:],t12= object_frame[:3], q12=object_frame[3:]\n",
    ")\n",
    "print(f\"combine_frame: {combine_frame}\")"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "my_env",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
